#pragma once

#include "../../Utility/Util/utility.hpp"
#include "../../Utility/Data/NavMessageData/GNSSMesData.hpp"
#include "../../Utility/Data/Position/CRDFactory.hpp"
#include "../../Utility/Data/Time/TimeFactory.hpp"
#include "../../Utility/Log/Log.hpp"

namespace ns_Calculate
{
    using namespace ns_GNSSMesData;
    using namespace ns_Time;
    using namespace ns_Util;
    using namespace ns_Log;
    using namespace ns_Position;

    class NavParameterCalculate
    {
    protected:
        struct Parameter
        {
            double sqrtA;
            double angularVelocity;
            double tk;
            double Mk;
            double Ek;
            double Vk;
            double argumentOfLatitude;
            double Cuk;
            double Crk;
            double Cik;
            double Uk;
            double Rk;
            double Ik;

            double OMGk;
        };

    public:
        NavParameterCalculate(const GNSSMesData::Ptr& input,const TimeFactory::TimePtr& curTime)
        {
            _input = input;
            _curTime = curTime;
            _gnssType = input->GetGNSSType();

            switch (_gnssType)
            {
            case GNSSType::BDS:
                InitParameterOfBDS(input);
                break;
            case GNSSType::GPS:
                InitParameterOfGPS(input);
                break;
            default:
                Log(Debug)<<"Can not handle this type of GNSS"<<'\n';
                break;
            }
        }

    protected:
        Parameter _parameter;
        GNSSType _gnssType;
        GNSSMesData::Ptr _input;
        TimeFactory::TimePtr _curTime;

    private:
        void InitParameterOfGPS(GNSSMesData::Ptr input)
        {
            auto inputHead = input->GetHeader();
            auto inputRecord = input->GetRecord();

            // 计算轨道长半轴
            {
                _parameter.sqrtA = inputRecord->sqrtA;
                _parameter.angularVelocity = Mathf::Sqrt(Mathf::uGPS) / Mathf::Pow(_parameter.sqrtA, 3);
            }

            // 计算相对于星历参考历元的时间
            {
                // 因为没有考虑信号传播时间，最终有小部分误差
                auto bt = TimeFactory::MakeTimePtr(TimeFactory::ValueType::GPSTimeSys,inputRecord->GPSWeek,inputRecord->TOE);

                _parameter.tk = _curTime->SecondFromRefTime() - bt->SecondFromRefTime();
                if (_parameter.tk > 604800 / 2)
                    _parameter.tk -= 604800;
                else if (_parameter.tk < -604800 / 2)
                    _parameter.tk += 604800;
            }

            // 对平均运动角速度进行改正
            {
                _parameter.angularVelocity += inputRecord->deltan;
            }

            // 计算平近点角
            {
                _parameter.Mk = inputRecord->M0 + _parameter.tk * _parameter.angularVelocity;
            }

            // 计算偏近点角
            {
                double Mk = _parameter.Mk;
                auto GetEk = [inputRecord, Mk](double Ep)
                {
                    return Ep - (Ep - inputRecord->e * Mathf::Sin(Ep) - Mk) / (1 - inputRecord->e * Mathf::Cos(Ep));
                };

                double E0 = _parameter.Mk;
                double E1 = GetEk(E0);
                while (abs(E1 - E0) >= 1E-14)
                {
                    E0 = E1;
                    E1 = GetEk(E0);
                }

                _parameter.Ek = E1;
            }

            // 计算真近点角
            {
                _parameter.Vk = Mathf::Arctan((Mathf::Sqrt(1 - inputRecord->e * inputRecord->e) * Mathf::Sin(_parameter.Ek)) / (Mathf::Cos(_parameter.Ek) - inputRecord->e));

                if (Mathf::Abs(_parameter.Vk - _parameter.Mk) > Mathf::Pi / 2)
                    _parameter.Vk += _parameter.Vk > _parameter.Mk ? -Mathf::Pi : Mathf::Pi;
            }

            // 计算升交角距(未经改正的)
            {
                _parameter.argumentOfLatitude = _parameter.Vk + inputRecord->omega;
            }

            // 计算二阶调和改正数
            {
                _parameter.Cuk = inputRecord->cus * Mathf::Sin(2 * _parameter.argumentOfLatitude) +
                                 inputRecord->cuc * Mathf::Cos(2 * _parameter.argumentOfLatitude);
                _parameter.Crk = inputRecord->crs * Mathf::Sin(2 * _parameter.argumentOfLatitude) +
                                 inputRecord->crc * Mathf::Cos(2 * _parameter.argumentOfLatitude);
                _parameter.Cik = inputRecord->cis * Mathf::Sin(2 * _parameter.argumentOfLatitude) +
                                 inputRecord->cic * Mathf::Cos(2 * _parameter.argumentOfLatitude);
            }

            // 计算经过改正的升交角距
            {
                _parameter.Uk = _parameter.argumentOfLatitude + _parameter.Cuk;
            }

            // 计算经过改正的向径
            {
                _parameter.Rk = _parameter.sqrtA * _parameter.sqrtA * (1 - inputRecord->e * Mathf::Cos(_parameter.Ek)) + _parameter.Crk;
            }

            // 计算经过改正的轨道倾角
            {
                _parameter.Ik = inputRecord->i0 + _parameter.Cik + inputRecord->IDOT * _parameter.tk;
            }

            // 计算改正后的升交点经度
            {
                _parameter.OMGk = inputRecord->OMEGA + (inputRecord->deltaOmega - Mathf::omgeGPS) * _parameter.tk - Mathf::omgeGPS * inputRecord->TOE;
            }
        }

        void InitParameterOfBDS(GNSSMesData::Ptr input)
        {
            auto inputHead = input->GetHeader();
            auto inputRecord = input->GetRecord();

            // 计算轨道长半轴
            {
                _parameter.sqrtA = inputRecord->sqrtA;
                _parameter.angularVelocity = Mathf::Sqrt(Mathf::uBDS) / Mathf::Pow(_parameter.sqrtA, 3);
            }

            // 计算相对于星历参考历元的时间
            {
                // 因为没有考虑信号传播时间，最终有小部分误差
                auto bt = TimeFactory::MakeTimePtr(TimeFactory::ValueType::GPSTimeSys,inputRecord->GPSWeek,inputRecord->TOE);

                _parameter.tk = _curTime->SecondFromRefTime() - bt->SecondFromRefTime();
                if (_parameter.tk > 604800 / 2)
                    _parameter.tk -= 604800;
                else if (_parameter.tk < -604800 / 2)
                    _parameter.tk += 604800;
            }

            // 对平均运动角速度进行改正
            {
                _parameter.angularVelocity += inputRecord->deltan;
            }

            // 计算平近点角
            {
                _parameter.Mk = inputRecord->M0 + _parameter.tk * _parameter.angularVelocity;
            }

            // 计算偏近点角
            {
                double Mk = _parameter.Mk;
                auto GetEk = [inputRecord, Mk](double Ep)
                {
                    return Ep - inputRecord->e * Mathf::Sin(Ep);
                };

                double E0 = _parameter.Mk;
                double E1 = GetEk(E0);
                while (abs(E1 - E0) >= 1E-12)
                {
                    E0 = E1;
                    E1 = GetEk(E0);
                }

                _parameter.Ek = E1;
            }

            // 计算真近点角
            {
                _parameter.Vk = Mathf::Arctan((Mathf::Sqrt(1 - inputRecord->e * inputRecord->e) * Mathf::Sin(_parameter.Ek)) / (Mathf::Cos(_parameter.Ek) - inputRecord->e));

                if (Mathf::Abs(_parameter.Vk - _parameter.Mk) > Mathf::Pi / 2)
                    _parameter.Vk += _parameter.Vk > _parameter.Mk ? -Mathf::Pi : Mathf::Pi;
            }

            // 计算升交角距(未经改正的)
            {
                _parameter.argumentOfLatitude = _parameter.Vk + inputRecord->omega;
            }

            // 计算二阶调和改正数
            {
                _parameter.Cuk = inputRecord->cus * Mathf::Sin(2 * _parameter.argumentOfLatitude) +
                                 inputRecord->cuc * Mathf::Cos(2 * _parameter.argumentOfLatitude);
                _parameter.Crk = inputRecord->crs * Mathf::Sin(2 * _parameter.argumentOfLatitude) +
                                 inputRecord->crc * Mathf::Cos(2 * _parameter.argumentOfLatitude);
                _parameter.Cik = inputRecord->cis * Mathf::Sin(2 * _parameter.argumentOfLatitude) +
                                 inputRecord->cic * Mathf::Cos(2 * _parameter.argumentOfLatitude);
            }

            // 计算经过改正的升交角距
            {
                _parameter.Uk = _parameter.argumentOfLatitude + _parameter.Cuk;
            }

            // 计算经过改正的向径
            {
                _parameter.Rk = _parameter.sqrtA * _parameter.sqrtA * (1 - inputRecord->e * Mathf::Cos(_parameter.Ek)) + _parameter.Crk;
            }

            // 计算经过改正的轨道倾角
            {
                _parameter.Ik = inputRecord->i0 + _parameter.Cik + inputRecord->IDOT * _parameter.tk;
            }

            // 计算改正后的升交点经度
            {
                if (inputRecord->serial <= 5 && inputRecord->serial >= 1)
                    _parameter.OMGk = inputRecord->OMEGA + inputRecord->deltaOmega * _parameter.tk - Mathf::omgeBDS * inputRecord->TOE;
                else
                    _parameter.OMGk = inputRecord->OMEGA + (inputRecord->deltaOmega - Mathf::omgeBDS) * _parameter.tk - Mathf::omgeBDS * inputRecord->TOE;
            }
        }
    };
}